import logging
import binascii

logger = logging.getLogger(__name__)

class ServoProtocol:
    """
    舵机通信协议类，负责生成与解析舵机控制协议
    """
    # 帧头和帧尾
    FRAME_HEADER = bytes([0xAA, 0xBB])
    FRAME_TAIL = bytes([0xCC, 0xDD])
    
    def __init__(self, supported_pins=None):
        """
        初始化舵机协议类
        
        Args:
            supported_pins (list): 支持的舵机引脚列表
        """
        self.supported_pins = supported_pins or [2, 3, 4, 6, 44, 45, 46, 7, 10]
    
    @staticmethod
    def calculate_checksum(data):
        """
        计算校验和
        
        Args:
            data (bytes): 需要计算校验和的数据
            
        Returns:
            bytes: 校验和（1字节）
        """
        return bytes([sum(data) & 0xFF])
    
    @staticmethod
    def angle_to_bytes(angle):
        """
        将角度转换为两个字节
        
        Args:
            angle (float): 角度值
            
        Returns:
            bytes: 表示角度的两个字节
        """
        angle_int = int(angle * 10)  # 放大10倍以保留一位小数
        return bytes([angle_int >> 8, angle_int & 0xFF])
    
    def generate_protocol(self, servo_data):
        """
        生成舵机控制协议数据
        
        Args:
            servo_data (dict): 舵机数据，格式为 {引脚: 角度}
        
        Returns:
            bytes: 协议数据
        """
        # 计算有效舵机数量
        servo_count = len(servo_data)
        if servo_count > 9:
            raise ValueError("舵机数量超过最大限制(9)")
        
        # 舵机数量
        data = bytearray([servo_count])
        
        # 添加舵机数据 (使用配置中的 supported_pins)
        supported_pins = self.supported_pins
        for i in range(9): # 假设协议固定为9个舵机位
            if i < len(supported_pins):
                pin = supported_pins[i]
                if pin in servo_data:
                    # 添加舵机引脚
                    data.extend([pin])
                    # 添加舵机角度
                    data.extend(self.angle_to_bytes(servo_data[pin]))
                else:
                    # 未使用的舵机位置填充0
                    data.extend([0, 0, 0])
            else:
                # 如果supported_pins不足9个，用0填充剩余位置
                data.extend([0, 0, 0])
        
        # 计算校验和
        checksum = self.calculate_checksum(self.FRAME_HEADER + data)
        
        # 组装完整协议数据
        protocol_data = self.FRAME_HEADER + data + checksum + self.FRAME_TAIL
        
        # 打印调试信息
        logger.debug(f"生成的协议数据: {protocol_data.hex()}")
        
        return protocol_data
    
    @staticmethod
    def generate_test_data():
        """
        生成测试数据
        
        Returns:
            bytes: 测试数据
        """
        return bytes.fromhex("AA BB 01 00 00 00 00 00 00 04 01 C2 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 2D CC DD")
